#include "motor_control.h"
#include "ch32v30x_gpio.h"
#include "ch32v30x_rcc.h"
#include "ch32v30x_tim.h"
#include <stdlib.h>

// 电机A
#define PWMA_GPIO_PORT GPIOA
#define PWMA_GPIO_PIN  GPIO_Pin_0

#define AIN1_GPIO_PORT GPIOB
#define AIN1_GPIO_PIN  GPIO_Pin_14
#define AIN2_GPIO_PORT GPIOB
#define AIN2_GPIO_PIN  GPIO_Pin_15

#define STBY_GPIO_PORT GPIOA
#define STBY_GPIO_PIN  GPIO_Pin_5


#define PWM_TIM TIM2
#define PWM_PERIOD 7999
#define PWM_CLK RCC_APB1Periph_TIM2

// 速度转换函数
uint16_t SpeedToPWM(uint8_t speed) {
    if(speed > 100) speed = 100;
    return (uint16_t)((uint32_t)speed * PWM_PERIOD / 100);
}

// 电机A初始化
void Motor_GPIO_Init(void) {
    GPIO_InitTypeDef GPIO_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
    RCC_APB1PeriphClockCmd(PWM_CLK, ENABLE);


    GPIO_InitStructure.GPIO_Pin = PWMA_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(PWMA_GPIO_PORT, &GPIO_InitStructure);


    GPIO_InitStructure.GPIO_Pin = AIN1_GPIO_PIN | AIN2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(AIN1_GPIO_PORT, &GPIO_InitStructure);


    GPIO_InitStructure.GPIO_Pin = STBY_GPIO_PIN;
    GPIO_Init(STBY_GPIO_PORT, &GPIO_InitStructure);
    GPIO_SetBits(STBY_GPIO_PORT, STBY_GPIO_PIN);
}


void Motor_PWM_Init(void) {
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_OCInitTypeDef TIM_OCInitStructure;

    TIM_TimeBaseStructure.TIM_Period = PWM_PERIOD;
    TIM_TimeBaseStructure.TIM_Prescaler = 8;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(PWM_TIM, &TIM_TimeBaseStructure);


    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 0;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC1Init(PWM_TIM, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(PWM_TIM, TIM_OCPreload_Enable);


    TIM_CtrlPWMOutputs(PWM_TIM, ENABLE);
    TIM_Cmd(PWM_TIM, ENABLE);

        TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);

        // 设置自动重装载值
        TIM_TimeBaseStructure.TIM_Period = 89; // 144MHz/(90*8)=200kHz, 10kHz PWM
}


void MotorA_Control(uint8_t direction, uint8_t speed) {
    uint16_t pwmValue = SpeedToPWM(speed);


    if(direction == 0) {  // 正转
        GPIO_ResetBits(AIN1_GPIO_PORT, AIN1_GPIO_PIN);
        GPIO_SetBits(AIN2_GPIO_PORT, AIN2_GPIO_PIN);
    } else {  // 反转
        GPIO_SetBits(AIN1_GPIO_PORT, AIN1_GPIO_PIN);
        GPIO_ResetBits(AIN2_GPIO_PORT, AIN2_GPIO_PIN);
    }


    TIM_SetCompare1(PWM_TIM, pwmValue);
}


void MotorA_Brake(void) {
    GPIO_SetBits(AIN1_GPIO_PORT, AIN1_GPIO_PIN);
    GPIO_SetBits(AIN2_GPIO_PORT, AIN2_GPIO_PIN);
    TIM_SetCompare1(PWM_TIM, 0);
}

// 电机B初始化
void MotorB_Init(void) {
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_OCInitTypeDef TIM_OCInitStructure;

    // 1. 启用时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    RCC_APB1PeriphClockCmd(PWM_CLK_B, ENABLE);

    // 2. 配置PWMB引脚 (PB0 - TIM3_CH3)
    GPIO_InitStructure.GPIO_Pin = PWMB_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(PWMB_GPIO_PORT, &GPIO_InitStructure);

    // 3. 配置方向引脚 (PB1, PB10)
    GPIO_InitStructure.GPIO_Pin = BIN1_GPIO_PIN | BIN2_GPIO_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(BIN1_GPIO_PORT, &GPIO_InitStructure);


    TIM_TimeBaseStructure.TIM_Period = PWM_PERIOD;
    TIM_TimeBaseStructure.TIM_Prescaler = 8;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(PWM_TIM_B, &TIM_TimeBaseStructure);


    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 0;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OC3Init(PWM_TIM_B, &TIM_OCInitStructure);

    //启用功能
    TIM_OC3PreloadConfig(PWM_TIM_B, TIM_OCPreload_Enable);
    TIM_CtrlPWMOutputs(PWM_TIM_B, ENABLE);
    TIM_Cmd(PWM_TIM_B, ENABLE);
}

// 电机B控制函数
void MotorB_Control(uint8_t direction, uint8_t speed) {
    // 设置方向
    if(direction == MOTOR_FORWARD) {
        GPIO_ResetBits(BIN1_GPIO_PORT, BIN1_GPIO_PIN);
        GPIO_SetBits(BIN2_GPIO_PORT, BIN2_GPIO_PIN);
    } else {
        GPIO_SetBits(BIN1_GPIO_PORT, BIN1_GPIO_PIN);
        GPIO_ResetBits(BIN2_GPIO_PORT, BIN2_GPIO_PIN);
    }

    // 设置PWM
    uint16_t pwmValue = SpeedToPWM(speed);
    TIM_SetCompare3(PWM_TIM_B, pwmValue);
}

// 电机B制动函数
void MotorB_Brake(void) {
    GPIO_SetBits(BIN1_GPIO_PORT, BIN1_GPIO_PIN);
    GPIO_SetBits(BIN2_GPIO_PORT, BIN2_GPIO_PIN);
    TIM_SetCompare3(PWM_TIM_B, 0);  // PWM清零
}

